#include <boustrophedon_explorator.h>

//#define DEBUG_VISUALIZATION

float wrapMax(float x, float max)
{
	/* integer math: `(max + x % max) % max` */
	return fmod(max + fmod(x, max), max);
}
/* wrap x -> [min,max) */
float wrapMinMax(float x, float min, float max)
{
	return min + wrapMax(x - min, max - min);
}

float wrapAngle(float yaw)
{
	return wrapMinMax(yaw, -M_PI, M_PI);
}

// Constructor
BoustrophedonExplorer::BoustrophedonExplorer()
{
}

std::vector<ipa_building_msgs::Point_x_y_theta> BoustrophedonExplorer::Get_Hybrid_path(cv::Mat map_expansion, std::vector<double> START, std::vector<double> GOAL)
{
	// std::string image_path;
	// image_path = "/home/zy/Desktop/CCPP_backup/boustrophedo/fov_path_map.png";
	// cv::Mat map = cv::imread(image_path, 0);
	cv::Mat map = map_expansion;
	std::cout << "map.rows  " << map.rows << "  map.cols  " << map.cols << std::endl;
	// 一楼商场
	for (int y = 0; y < map.rows; y++)
	{
		for (int x = 0; x < map.cols; x++)
		{
			//find not reachable regions and make them black
			if (map.at<unsigned char>(y, x) == 255)
			{
				map.at<unsigned char>(y, x) = 255; // 白
			}
			//else make it white
			else
			{
				map.at<unsigned char>(y, x) = 0; // 黑
			}
		}
	}

	cv::Mat room_map_hybrid = map;
	std::vector<ipa_building_msgs::Point_x_y_theta> hybrid_path;

	int persist_map_hybrid_A_star[room_map_hybrid.cols][room_map_hybrid.rows];
	vector<vector<int>> gird_map(room_map_hybrid.cols, vector<int>(room_map_hybrid.rows));

	for (int y = 0; y < room_map_hybrid.rows; y++)
	{
		for (int x = 0; x < room_map_hybrid.cols; x++)
		{
			//find not reachable regions and make them black
			if (room_map_hybrid.at<unsigned char>(y, x) == 255)
			{
				// map.at<unsigned char>(y, x) = 255; // 白
				persist_map_hybrid_A_star[x][y] = 255;
				gird_map[x][y] = 0; // 没有障碍物
			}
			//else make it white
			else
			{
				persist_map_hybrid_A_star[x][y] = 0; // 黑
				gird_map[x][y] = 1;					 // 有障碍物
			}
		}
	}

	HBF hbf = HBF();

	// std::vector<double> START = {start_point.x, start_point.y, 0.0};
	// std::vector<double> GOAL = {goal_point.x, goal_point.y, 0};
	// std::cout << "start x, y  " << start_point.x << " " << start_point.y << std::endl;
	// std::cout << "goal_point x, y  " << goal_point.x << " " << goal_point.y << std::endl;

	// std::vector<double> START = {100, 100, 0.0};
	// std::vector<double> GOAL = {500, 500, 0};

	// std::vector<double> START = {1264, 390, 0.0};
	// std::vector<double> GOAL = {397, 397, 0};
	// std::cout << "start x,y " << START[0] << "  " << START[1] << std::endl;
	// std::cout << "goal x,y " << GOAL[0] << "  " << GOAL[1] << std::endl;

	HBF::maze_path get_path = hbf.search(gird_map, START, GOAL);

	// 最终的路径点
	vector<HBF::maze_s> show_path = hbf.reconstruct_path(get_path.came_from, START, get_path.final);

	for (int p = show_path.size() - 1; p >= 0; p--)
	{
		ipa_building_msgs::Point_x_y_theta tmp_pose;
		// to_do
		tmp_pose.x = show_path[p].x; // grid_to_pose
		tmp_pose.y = show_path[p].y;
		tmp_pose.theta = show_path[p].theta;
		hybrid_path.push_back(tmp_pose);
	}

	std::cout << "hybrid_path.size()   " << hybrid_path.size() << std::endl;
	return hybrid_path;
}

// Function that creates a room exploration path for the given map, by using the morse cellular decomposition method proposed in
//
// "H. Choset, E. Acar, A. A. Rizzi and J. Luntz,
// "Exact cellular decompositions in terms of critical points of Morse functions," Robotics and Automation, 2000. Proceedings.
// ICRA '00. IEEE International Conference on, San Francisco, CA, 2000, pp. 2270-2277 vol.3."
//
// This method takes the given map and separates it into several cells. Each cell is obstacle free and so allows an
// easier path planning. For each cell then a boustrophedon path is planned, which goes up, down and parallel to the
// upper and lower boundaries of the cell, see the referenced paper for details. This function does the following steps:
// I.	Using the Sobel operator the direction of the gradient at each pixel is computed. Using this information, the direction is
//		found that suits best for calculating the cells, i.e. such that longer cells occur, and the map is rotated in this manner.
//		This allows to use the algorithm as it was and in the last step, the found path points simply will be transformed back to the
//		original orientation.
// II.	Sweep a slice (a morse function) trough the given map and check for connectivity of this line,
//		i.e. how many connected segments there are. If the connectivity increases, i.e. more segments appear,
//		an IN event occurs that opens new separate cells, if it decreases, i.e. segments merge, an OUT event occurs that
//		merges two cells together. If an event occurs, the algorithm checks along the current line for critical points,
//		that are points that trigger the events. From these the boundary of the cells are drawn, starting from the CP
//		and going left/right until a black pixel is hit.
// III.	After all cells have been determined by the sweeping slice, the algorithm finds these by using cv::findContours().
//		This gives a set of points for each cell, that are used to create a generalizedPolygon out of each cell.
// IV.	After all polygons have been created, plan the path trough all of them for the field of view s.t. the whole area
//		is covered. To do so, first a global path trough all cells is generated, using the traveling salesmen problem
//		formulation. This produces an optimal visiting order of the cells. Next for each cell a boustrophedon path is
//		determined, which goes back and forth trough the cell and between the horizontal paths along the boundaries of
//		the cell, what ensures that the whole area of the cell is covered. For each cell the longest edge is found and it is transformed
//		s.t. this edge lies horizontal to the x-direction. This produces longer but fewer edges, what improves the path for small but long
//		cells. The startpoint of the cell-path is determined by the endpoint of the previous cell, s.t. the distance between two
//		cell-paths is minimized. The cell-path is determined in the rotated manner, so in a last step, the cell-path is transformed to
//		the originally transformed cell and after that inserted in the global path.
// V.	The previous step produces a path for the field of view. If wanted this path gets mapped to the robot path s.t.
//		the field of view follows the wanted path. To do so simply a vector transformation is applied. If the computed robot
//		pose is not in the free space, another accessible point is generated by finding it on the radius around the field of view (fov)
//		middlepoint s.t. the distance to the last robot position is minimized. If this is not wanted one has to set the
//		corresponding Boolean to false (shows that the path planning should be done for the robot footprint).
// room_map = expects to receive the original, not inflated room map
void BoustrophedonExplorer::getExplorationPath(const cv::Mat &room_map, std::vector<ipa_building_msgs::Point_x_y_theta> &path,
											   const float map_resolution, const cv::Point starting_position, const cv::Point2d map_origin,
											   double grid_spacing_in_pixel, const double grid_obstacle_offset, const double path_eps,
											   int cell_visiting_order,
											   const bool plan_for_footprint, const Eigen::Matrix<float, 2, 1> robot_to_fov_vector,
											   const double min_cell_area,
											   const int max_deviation_from_track, const float min_line_cleaning_distance,
											   int complementary_path_index, int need_hybrid_index,
											   int index_to_boundary)
{
	std::cout << "Planning the boustrophedon path trough the room." << std::endl;
	// debug code
	grid_spacing_in_pixel = 10;
	int grid_spacing_as_int = (int)std::floor(grid_spacing_in_pixel);			 // convert fov-radius to int
	int half_grid_spacing_as_int = (int)std::floor(0.5 * grid_spacing_in_pixel); // convert fov-radius to int
	int min_cell_width = half_grid_spacing_as_int + 2. * grid_obstacle_offset / map_resolution;
	float min_line_cleaning_distance_ = min_line_cleaning_distance;

	//  1. Find the main directions of the map and rotate it in this manner
	//  2. Sweep a slice trough the map and mark the found cell boundaries
	//  3. Find the separated cells
	cv::Mat R;
	cv::Rect bbox;
	cv::Mat rotated_room_map;
	std::vector<GeneralizedPolygon> cell_polygons;
	std::vector<cv::Point> polygon_centers;
	computeCellDecompositionWithRotation(room_map, map_resolution, min_cell_area, min_cell_width, 0., R,
										 bbox, rotated_room_map, cell_polygons, polygon_centers);
	// does not work so well:
	// findBestCellDecomposition(room_map, map_resolution, min_cell_area, R, bbox, rotated_room_map, cell_polygons, polygon_centers);

	std::cout << "Found the cells in the given map. " << std::endl;

	// IV. Determine the cell paths.
	// determine the start cell that contains the start position
	std::vector<cv::Point> starting_point_vector(1, starting_position); // opencv syntax
	cv::transform(starting_point_vector, starting_point_vector, R);
	const cv::Point rotated_starting_point = starting_point_vector[0]; // point that keeps track of the last point after the boustrophedon path in each cell

	//	testing
	//	cv::Mat center_map = room_map.clone();
	//	for(size_t i=0; i<cell_polygons.size(); ++i)
	//		cv::circle(center_map, cell_polygons[i].getCenter(), 2, cv::Scalar(127), CV_FILLED);
	//	cv::circle(center_map, starting_position, 4, cv::Scalar(100), CV_FILLED);
	//	cv::imshow("centers", center_map);
	//	cv::waitKey();

	int start_cell_index = 0;
	for (std::vector<GeneralizedPolygon>::iterator cell = cell_polygons.begin(); cell != cell_polygons.end(); ++cell)
	{
		if (cv::pointPolygonTest(cell->getVertices(), rotated_starting_point, false) >= 0)
		{
			start_cell_index = cell - cell_polygons.begin();
		}
	}

	// determine the visiting order of the cells
	// cell_polygon的最优访问顺序
	std::vector<int> optimal_order;
	// 对每个cell的访问顺序
	// cell_visiting_order = OPTIMAL_TSP;
	cell_visiting_order = LEFT_TO_RIGHT;
	if (cell_visiting_order == OPTIMAL_TSP)
	{
		// determine the optimal visiting order of the cells
		//	ConcordeTSPSolver tsp_solver;
		//	std::vector<int> optimal_order = tsp_solver.solveConcordeTSP(rotated_room_map, polygon_centers, 0.25, 0.0, map_resolution, start_cell_index, 0);
		GeneticTSPSolver tsp_solver;
		optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 0.25, 0.0, map_resolution, start_cell_index, 0);
		if (optimal_order.size() != polygon_centers.size())
		{
			std::cout << "=====================> Genetic TSP failed with 25% resolution, falling back to 100%. <=======================" << std::endl;
			optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 1.0, 0.0, map_resolution, start_cell_index, 0);
		}
	}
	else if (cell_visiting_order == LEFT_TO_RIGHT)
	{
		// we are using an alternative ordering here, which visits the cells in a more obvious fashion to the human observer (though it is not optimal)
		std::multimap<int, int> y_coordinate_ordering; // <y-coordinate of room centers, index>
		for (size_t i = 0; i < polygon_centers.size(); ++i)
			y_coordinate_ordering.insert(std::pair<int, int>(polygon_centers[i].y, (int)i));
		for (std::multimap<int, int>::iterator it = y_coordinate_ordering.begin(); it != y_coordinate_ordering.end(); ++it)
			optimal_order.push_back(it->second);
	}
	else
	{
		std::cout << "Error: BoustrophedonExplorer::getExplorationPath: The specified cell_visiting_order=" << cell_visiting_order << " is invalid." << std::endl;
		return;
	}

	///////////////////////////////////////////////////////////////////////
	// go trough the cells [in optimal visiting order] and determine the boustrophedon paths
	std::cout << "Starting to get the paths for each cell, number of cells: " << (int)cell_polygons.size() << std::endl;

	// 修改弓字形的间隔
	std::cout << "Boustrophedon grid_spacing_as_int=" << grid_spacing_as_int << std::endl;
	cv::Point robot_pos = rotated_starting_point;  // point that keeps track of the last point after the boustrophedon path in each cell
	std::vector<cv::Point2f> fov_middlepoint_path; // this is the trajectory of centers of the robot footprint or the field of view

	// 对于每一个cell_polygons 细胞多边形进行路径搜索
	for (size_t cell = 0; cell < cell_polygons.size(); ++cell)
	// for (size_t cell = 15; cell < cell_polygons.size(); ++cell)
	// size_t cell = 12;
	{

		std::cout << "cell:  " << cell << "   optimal_order[cell]:  " << optimal_order[cell] << std::endl;
		computeBoustrophedonPath(rotated_room_map, map_resolution, cell_polygons[optimal_order[cell]], cell,
								 fov_middlepoint_path,
								 robot_pos, grid_spacing_as_int, half_grid_spacing_as_int, path_eps,
								 max_deviation_from_track, grid_obstacle_offset / map_resolution,
								 min_line_cleaning_distance_, index_to_boundary);
		std::cout << "fov_middlepoint_path.size()  " << fov_middlepoint_path.size() << std::endl;
	}

	cv::Point robot_pos_miss_out;
	robot_pos_miss_out.x = fov_middlepoint_path.back().x;
	robot_pos_miss_out.y = fov_middlepoint_path.back().y;
	computeBoustrophedonPath_miss_out(rotated_room_map, map_resolution, cell_polygons, optimal_order,
									  fov_middlepoint_path,
									  robot_pos_miss_out, grid_spacing_as_int, half_grid_spacing_as_int, path_eps,
									  max_deviation_from_track, grid_obstacle_offset / map_resolution,
									  min_line_cleaning_distance_);
	/////////////////////////////////////////////////////////////////
	// transform the calculated path back to the originally rotated map and create poses with an angle
	RoomRotator room_rotation;
	// fov_poses: this is the trajectory of poses of the robot footprint or the field of view, in [pixels]
	std::vector<ipa_building_msgs::Point_x_y_theta> fov_poses;
	// 转换
	room_rotation.transformPathBackToOriginalRotation(fov_middlepoint_path, fov_poses, R);

	std::cout << "fov_poses.size()  " << fov_poses.size() << std::endl;

	// 添加 Hybrid A*的路径点
	if (0)
	{
		std::vector<ipa_building_msgs::Point_x_y_theta> hybrid_path_debug;
		int distance_need_hybrid = need_hybrid_index;
		std::cout << "distance_need_hybrid   " << distance_need_hybrid << std::endl;
		int count = 0;
		for (int i = 1; i < fov_poses.size(); i++)
		{
			float y_diff = fov_poses[i - 1].y - fov_poses[i].y;
			float x_diff = fov_poses[i - 1].x - fov_poses[i].x;
			float distance = hypot(y_diff, x_diff);
			if (distance > distance_need_hybrid) // 距离大于100个index
			{
				// std::cout << "distance > 100  " << std::endl;
				count++;
			}
		}
		std::cout << "count   " << count << std::endl;

		for (int k = 0; k < count; k++)
		{
			int current_index = 0;
			for (int i = 1; i < fov_poses.size(); i++)
			{
				float y_diff = fov_poses[i - 1].y - fov_poses[i].y;
				float x_diff = fov_poses[i - 1].x - fov_poses[i].x;
				float distance = hypot(y_diff, x_diff);
				if (distance > distance_need_hybrid) // 距离大于100个index
				{
					// std::cout << "distance > 100  " << std::endl;
					current_index = i;
					break;
					// too_far_index.push_back(i);
				}
			}

			auto goal_point = fov_poses[current_index];
			auto start_point = fov_poses[current_index - 1];

			std::vector<double> START = {start_point.x, start_point.y, start_point.theta};
			std::vector<double> GOAL = {goal_point.x, goal_point.y, goal_point.theta};

			std::cout << "start x,y,theta " << START[0] << "  " << START[1] << " " << START[2] << std::endl;
			std::cout << "goal x,y,theta " << GOAL[0] << "  " << GOAL[1] << " " << GOAL[2] << std::endl;
			std::vector<ipa_building_msgs::Point_x_y_theta> hybrid_path = Get_Hybrid_path(room_map, START, GOAL);
			// // Hybrid A*的角度
			// for (int hybrid_path_index = 0; hybrid_path_index < hybrid_path.size(); hybrid_path_index++)
			// {
			// 	hybrid_path[hybrid_path_index].theta = wrapAngle(hybrid_path[hybrid_path_index].theta);
			// 	float theta_ccpp_x = -0.359602;
			// 	hybrid_path[hybrid_path_index].theta = 1.57 + (hybrid_path[hybrid_path_index].theta + theta_ccpp_x) + theta_ccpp_x;
			// }

			// 预留START和GOAL各20个index的距离
			// Hybrid A* 搜索出的路线的长度
			float y_diff_hybrid = hybrid_path.front().y - hybrid_path.back().y;
			float x_diff_hybrid = hybrid_path.front().x - hybrid_path.back().x;
			float distance_hybrid = hypot(y_diff_hybrid, x_diff_hybrid);
			std::cout << "distance_hybrid  " << distance_hybrid << std::endl;
			// if (distance_hybrid > complementary_path_index * 2)

			// 距离 START 的距离等于20个index的点
			int start_index_hybrid = 0;
			for (int start_index = 0; start_index < hybrid_path.size(); start_index++)
			{
				float y_diff_tmp = START[1] - hybrid_path[start_index].y;
				float x_diff_tmp = START[0] - hybrid_path[start_index].x;
				float distance_hybrid_tmp = hypot(y_diff_tmp, x_diff_tmp);
				if (distance_hybrid_tmp > complementary_path_index)
				{
					start_index_hybrid = start_index;
					break;
				}
			}

			// 距离 GOAL 的距离等于20个index的点
			int goal_index_hybrid = hybrid_path.size() - 1;
			for (int goal_index = hybrid_path.size() - 1; goal_index >= 0; goal_index--)
			{
				float y_diff_tmp = GOAL[1] - hybrid_path[goal_index].y;
				float x_diff_tmp = GOAL[0] - hybrid_path[goal_index].x;
				float distance_hybrid_tmp = hypot(y_diff_tmp, x_diff_tmp);
				if (distance_hybrid_tmp > complementary_path_index)
				{
					goal_index_hybrid = goal_index;
					break;
				}
			}

			std::cout << "start_index_hybrid  " << start_index_hybrid << std::endl;
			std::cout << "goal_index_hybrid  " << goal_index_hybrid << std::endl;

			if (start_index_hybrid < goal_index_hybrid)
			{
				fov_poses.insert((fov_poses.begin() + current_index), hybrid_path.begin() + start_index_hybrid,
								 hybrid_path.begin() + goal_index_hybrid);

				// fov_poses.insert((fov_poses.begin() + current_index), hybrid_path.begin(),
				// 				 hybrid_path.end());

				// hybrid_path_debug.insert(hybrid_path_debug.end(), hybrid_path.begin() + start_index_hybrid,
				// 						 hybrid_path.begin() + goal_index_hybrid);
				std::cout << "fov_poses.size()   " << fov_poses.size() << std::endl;
				std::cout << "current_index   " << current_index << std::endl;
			}
			else
			{
				// fov_poses.insert((fov_poses.begin() + current_index), hybrid_path.begin() + hybrid_path.size() / 3,
				// 				 hybrid_path.begin() + 2 * hybrid_path.size() / 3);
				// 当 Hybrid A*的路径点,比较少,一般是少于40个点,
				std::cout << "start_index_hybrid >= goal_index_hybrid" << std::endl;
				// fov_poses.insert((fov_poses.begin() + current_index), hybrid_path.begin() + hybrid_path.size() / 2,
				// 				 hybrid_path.begin() + hybrid_path.size() / 2 + 2);
				fov_poses.insert((fov_poses.begin() + current_index), hybrid_path.begin() + start_index_hybrid,
								 hybrid_path.end());
			}
		}
	}
	// 添加 Hybrid A*的路径点

	// // 读取文件
	// ifstream f;
	// // 读取CCPP产生的原始轨迹
	// f.open("/home/zy/Desktop/CCPP_backup/boustrophedo/fov_poses_hybrid.csv");

	// while (!f.eof())
	// {
	// 	std::string s;
	// 	getline(f, s);
	// 	if (!s.empty())
	// 	{
	// 		stringstream ss_1;
	// 		ss_1 << s;
	// 		float pose_x;
	// 		float pose_y;
	// 		float pose_theta;

	// 		ss_1 >> pose_x;
	// 		ss_1 >> pose_y;
	// 		// ss_1 >> pose_theta;
	// 		// 加入路径点中
	// 		ipa_building_msgs::Point_x_y_theta pose_single;
	// 		pose_single.x = float(pose_x);
	// 		pose_single.y = float(pose_y);
	// 		pose_single.theta = float(pose_theta);
	// 		fov_poses.push_back(pose_single);
	// 	}
	// }
	// f.close();

	// // 保存到.csv
	// // 画图脚本  plot_CCPP.py
	// {
	// 	std::ofstream trial_file_fov_poses;
	// 	trial_file_fov_poses.open("/home/zy/Desktop/CCPP_backup/boustrophedo/fov_poses.csv", std::ios::ate);
	// 	for (int i = 1; i < fov_poses.size(); i++)
	// 	{
	// 		trial_file_fov_poses << std::to_string(fov_poses[i].x) << "\t"
	// 							 << std::to_string(fov_poses[i].y) << "\t"
	// 							 << std::to_string(fov_poses[i].theta) << "\t" << std::endl;
	// 	}
	// 	trial_file_fov_poses.close();
	// }

	std::vector<ipa_building_msgs::Point_x_y_theta> ccpp_path_index;
	for (int i = 0; i < fov_poses.size(); i++)
	{
		ipa_building_msgs::Point_x_y_theta point_tmp;

		// 起点作为原点
		// y取反
		point_tmp.x = fov_poses[i].y - fov_poses[0].y;
		point_tmp.y = fov_poses[i].x - fov_poses[0].x; // 注意坐标系的转换
		point_tmp.x = point_tmp.x * (-1);
		point_tmp.y = point_tmp.y * (-1);
		// grid to pose
		point_tmp.x *= map_resolution;
		point_tmp.y *= map_resolution;
		// theta角的坐标转换
		point_tmp.theta = (fov_poses[i].theta);
		// point_tmp.theta = (point_tmp.theta > 3.14) ? 3.14 : point_tmp.theta;
		// point_tmp.theta = (point_tmp.theta < 0) ? 0 : point_tmp.theta;

		ccpp_path_index.push_back(point_tmp);
	}

	float x_diff = ccpp_path_index[10].x - ccpp_path_index[1].x;
	float y_diff = ccpp_path_index[10].y - ccpp_path_index[1].y;
	float theta_ccpp_x = atan(y_diff / x_diff);
	std::cout << "theta_ccpp_x   " << theta_ccpp_x << std::endl;
	for (int i = 0; i < ccpp_path_index.size(); i++)
	{
		ccpp_path_index[i].theta = wrapAngle(ccpp_path_index[i].theta);
		ccpp_path_index[i].theta = 1.57 + (ccpp_path_index[i].theta + theta_ccpp_x) + theta_ccpp_x;
	}

	// 保存到.csv
	// 画图脚本  plot_CCPP.py
	{
		std::ofstream trial_file;
		trial_file.open("/home/zy/Desktop/CCPP_backup/boustrophedo/CCPP_path_points_origin.csv", std::ios::ate);
		for (int i = 1; i < ccpp_path_index.size(); i++)
		{
			trial_file << std::to_string((int32_t)(1000 * ccpp_path_index[i].x)) << "\t"
					   << std::to_string((int32_t)(1000 * ccpp_path_index[i].y)) << "\t"
					   << std::to_string((int32_t)(1000 * ccpp_path_index[i].theta)) << "\t" << std::endl;

			// trial_file << std::to_string(ccpp_path_index[i].x) << "\t"
			// 		   << std::to_string(ccpp_path_index[i].y) << "\t"
			// 		   << std::to_string(ccpp_path_index[i].theta) << "\t" << std::endl;
		}
		trial_file.close();
	}

	// // hybrid_path_debug_transform
	// if (0)
	// {
	// 	std::vector<ipa_building_msgs::Point_x_y_theta> hybrid_path_debug_transform;
	// 	for (int i = 0; i < hybrid_path_debug.size(); i++)
	// 	{
	// 		ipa_building_msgs::Point_x_y_theta point_tmp;
	// 		// 起点作为原点
	// 		// y取反
	// 		point_tmp.x = hybrid_path_debug[i].y - fov_poses[0].y;
	// 		point_tmp.y = hybrid_path_debug[i].x - fov_poses[0].x; // 注意坐标系的转换
	// 		point_tmp.x = point_tmp.x * (-1);
	// 		point_tmp.y = point_tmp.y * (-1);
	// 		// grid to pose
	// 		point_tmp.x *= map_resolution;
	// 		point_tmp.y *= map_resolution;
	// 		// theta角的坐标转换
	// 		point_tmp.theta = (hybrid_path_debug[i].theta);
	// 		// point_tmp.theta = (point_tmp.theta > 3.14) ? 3.14 : point_tmp.theta;
	// 		// point_tmp.theta = (point_tmp.theta < 0) ? 0 : point_tmp.theta;

	// 		hybrid_path_debug_transform.push_back(point_tmp);
	// 	}
	// 	// Hybrid A* 角度
	// 	for (int i = 0; i < hybrid_path_debug_transform.size(); i++)
	// 	{
	// 		hybrid_path_debug_transform[i].theta = wrapAngle(hybrid_path_debug_transform[i].theta);
	// 		hybrid_path_debug_transform[i].theta = 1.57 + (hybrid_path_debug_transform[i].theta + theta_ccpp_x) + theta_ccpp_x;
	// 	}

	// 	// 保存到.csv
	// 	// 画图脚本  plot_CCPP.py
	// 	{
	// 		std::ofstream trial_file;
	// 		trial_file.open("/home/zy/Desktop/CCPP_backup/boustrophedo/hybrid_path_debug_transform.csv", std::ios::ate);
	// 		for (int i = 1; i < hybrid_path_debug_transform.size(); i++)
	// 		{
	// 			// trial_file << std::to_string((int32_t)(1000 * ccpp_path_index[i].x)) << "\t"
	// 			// 		   << std::to_string((int32_t)(1000 * ccpp_path_index[i].y)) << "\t"
	// 			// 		   << std::to_string((int32_t)(1000 * ccpp_path_index[i].theta)) << "\t" << std::endl;

	// 			trial_file << std::to_string(hybrid_path_debug_transform[i].x) << "\t"
	// 					   << std::to_string(hybrid_path_debug_transform[i].y) << "\t"
	// 					   << std::to_string(hybrid_path_debug_transform[i].theta) << "\t" << std::endl;
	// 		}
	// 		trial_file.close();
	// 	}
	// } // hybrid_path_debug_transform // 结束

	//  搜索到的路径点以栅格的索引表示: fov_poses
	for (std::vector<ipa_building_msgs::Point_x_y_theta>::iterator pose = fov_poses.begin(); pose != fov_poses.end(); ++pose)
	{
		ipa_building_msgs::Point_x_y_theta current_pose;
		current_pose.x = (pose->x * map_resolution) + map_origin.x;
		current_pose.y = (pose->y * map_resolution) + map_origin.y;
		current_pose.theta = pose->theta;
		path.push_back(current_pose);
	}

	std::cout << "fov_poses.size()  " << fov_poses.size() << std::endl;
	std::cout << "打印CCPP到地图上   path.size()  " << path.size() << std::endl;

	// // 简化轨迹点
	// int JPS_sequence = 0;
	// while (JPS_sequence < 4)
	// {
	// 	for (int i = 0; i < path.size(); i++)
	// 	{
	// 		for (int j = i + 1; j < path.size(); j++)
	// 		{
	// 			// float distance = sqrtf((powf((goal_point_tmp[j].y-goal_point_tmp[i].y),2)+powf((goal_point_tmp[j].x-goal_point_tmp[i].x),2)));
	// 			float distance = hypotf((path[j].y - path[i].y), (path[j].x - path[i].x));
	// 			// 删除距离近的点
	// 			if (distance < 0.01)
	// 			{
	// 				path.erase(path.begin() + j);
	// 			}
	// 		}
	// 	}
	// 	JPS_sequence++;
	// }

	// std::cout << "简化后的路径点   path.size()  " << path.size() << std::endl;

	//save image
	// cv::Mat cell_map = room_map.clone();
	// imwrite("/home/zy/Desktop/CCPP_backup/boustrophedo/fov_path_map.png", cell_map);

	// // 打印CCPP到地图上
	// cv::Mat fov_path_map = room_map.clone();
	// for (size_t i = 1; i < path.size(); ++i)
	// {
	// 	cv::Point p1((path[i - 1].x - map_origin.x) / map_resolution, (path[i - 1].y - map_origin.y) / map_resolution);
	// 	cv::Point p2((path[i].x - map_origin.x) / map_resolution, (path[i].y - map_origin.y) / map_resolution);
	// 	cv::circle(fov_path_map, p2, 0.2, cv::Scalar(100), CV_FILLED);
	// 	cv::namedWindow("cell path", CV_WINDOW_NORMAL); //CV_WINDOW_NORMAL就是0
	// 	cv::imshow("cell path", fov_path_map);
	// 	cv::waitKey(10);
	// }
	// cv::waitKey();

	return; 
}

void BoustrophedonExplorer::computeCellDecompositionWithRotation(const cv::Mat &room_map, const float map_resolution, const double min_cell_area,
																 const int min_cell_width, const double rotation_offset, cv::Mat &R, cv::Rect &bbox, cv::Mat &rotated_room_map,
																 std::vector<GeneralizedPolygon> &cell_polygons, std::vector<cv::Point> &polygon_centers)
{
	// *********************** I. Find the main directions of the map and rotate it in this manner. ***********************
	RoomRotator room_rotation;
	room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution, 0, rotation_offset);
	room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox);

#ifdef DEBUG_VISUALIZATION
//	// testing
//	cv::Mat room_map_disp = room_map.clone();
//	cv::circle(room_map_disp, starting_position, 3, cv::Scalar(160), CV_FILLED);
//	cv::imshow("room_map", room_map_disp);
//	std::vector<cv::Point> tester;
//	tester.push_back(cv::Point(10,10));
//	cv::Mat tester_map = room_map.clone();
//	cv::circle(tester_map, tester[0], 3, cv::Scalar(127), CV_FILLED);
//	cv::transform(tester, tester, R);
//	cv::circle(rotated_room_map, tester[0], 3, cv::Scalar(127), CV_FILLED);
//	cv::imshow("original", tester_map);
//	cv::imshow("rotated_im.png", rotated_room_map);
//	cv::waitKey();
#endif

	// *********************** II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
	// *********************** III. Find the separated cells. ***********************
	computeCellDecomposition(rotated_room_map, map_resolution, min_cell_area, min_cell_width, cell_polygons, polygon_centers);
}

void BoustrophedonExplorer::computeCellDecomposition(const cv::Mat &room_map, const float map_resolution, const double min_cell_area,
													 const int min_cell_width, std::vector<GeneralizedPolygon> &cell_polygons, std::vector<cv::Point> &polygon_centers)
{
	// *********************** II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
	// create a map copy to mark the cell boundaries
	cv::Mat cell_map = room_map.clone();

	// find smallest y-value for that a white pixel occurs, to set initial y value and find initial number of segments
	size_t y_start = 0;
	bool found = false, obstacle = false;
	int previous_number_of_segments = 0;
	std::vector<int> previous_obstacles_end_x; // keep track of the end points of obstacles
	for (size_t y = 0; y < room_map.rows; ++y)
	{
		for (size_t x = 0; x < room_map.cols; ++x)
		{
			if (found == false && room_map.at<uchar>(y, x) == 255)
			{
				y_start = y;
				found = true;
			}
			else if (found == true && obstacle == false && room_map.at<uchar>(y, x) == 0)
			{
				++previous_number_of_segments;
				obstacle = true;
			}
			else if (found == true && obstacle == true && room_map.at<uchar>(y, x) == 255)
			{
				obstacle = false;
				previous_obstacles_end_x.push_back(x);
			}
		}

		if (found == true)
			break;
	}

	// sweep trough the map and detect critical points
	for (size_t y = y_start + 1; y < room_map.rows; ++y) // start at y_start+1 because we know number of segments at y_start
	{
		int number_of_segments = 0; // int to count how many segments at the current slice are
		std::vector<int> current_obstacles_start_x;
		std::vector<int> current_obstacles_end_x;
		bool obstacle_hit = false;	  // bool to check if the line currently hit an obstacle, s.t. not all black pixels trigger an event
		bool hit_white_pixel = false; // bool to check if a white pixel has been hit at the current slice, to start the slice at the first white pixel

		// count number of segments within this row
		for (size_t x = 0; x < room_map.cols; ++x)
		{
			if (hit_white_pixel == false && room_map.at<uchar>(y, x) == 255)
				hit_white_pixel = true;
			else if (hit_white_pixel == true)
			{
				if (obstacle_hit == false && room_map.at<uchar>(y, x) == 0) // check for obstacle
				{
					++number_of_segments;
					obstacle_hit = true;
					current_obstacles_start_x.push_back(x);
				}
				else if (obstacle_hit == true && room_map.at<uchar>(y, x) == 255) // check for leaving obstacle
				{
					obstacle_hit = false;
					current_obstacles_end_x.push_back(x);
				}
			}
		}

		// if the number of segments did not change, check whether the position of segments has changed so that there is a gap between them
		bool segment_shift_detected = false;
		if (previous_number_of_segments == number_of_segments && current_obstacles_start_x.size() == previous_obstacles_end_x.size() + 1)
		{
			for (size_t i = 0; i < previous_obstacles_end_x.size(); ++i)
				if (current_obstacles_start_x[i] > previous_obstacles_end_x[i])
				{
					segment_shift_detected = true;
					break;
				}
		}

		// reset hit_white_pixel to use this Boolean later
		hit_white_pixel = false;

		// check if number of segments has changed --> event occurred
		if (previous_number_of_segments < number_of_segments || segment_shift_detected == true) // IN event (or shift)
		{
			// check the current slice again for critical points
			for (int x = 0; x < room_map.cols; ++x)
			{
				if (hit_white_pixel == false && room_map.at<uchar>(y, x) == 255)
					hit_white_pixel = true;
				else if (hit_white_pixel == true && room_map.at<uchar>(y, x) == 0)
				{
					// check over black pixel for other black pixels, if none occur a critical point is found
					bool critical_point = true;
					for (int dx = -1; dx <= 1; ++dx)
						if (room_map.at<uchar>(y - 1, std::max(0, std::min(x + dx, room_map.cols - 1))) == 0)
							critical_point = false;

					// if a critical point is found mark the separation, note that this algorithm goes left and right
					// starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
					// behind other obstacles on the same y-value as the critical point
					if (critical_point == true)
					{
						// to the left until a black pixel is hit
						for (int dx = -1; x + dx >= 0; --dx)
						{
							uchar &val = cell_map.at<uchar>(y, x + dx);
							if (val == 255 && cell_map.at<uchar>(y - 1, x + dx) == 255)
								val = BORDER_PIXEL_VALUE;
							else if (val == 0)
								break;
						}

						// to the right until a black pixel is hit
						for (int dx = 1; x + dx < room_map.cols; ++dx)
						{
							uchar &val = cell_map.at<uchar>(y, x + dx);
							if (val == 255 && cell_map.at<uchar>(y - 1, x + dx) == 255)
								val = BORDER_PIXEL_VALUE;
							else if (val == 0)
								break;
						}
					}
				}
			}
		}
		else if (previous_number_of_segments > number_of_segments) // OUT event
		{
			// check the previous slice again for critical points --> y-1
			for (int x = 0; x < room_map.cols; ++x)
			{
				if (room_map.at<uchar>(y - 1, x) == 255 && hit_white_pixel == false)
					hit_white_pixel = true;
				else if (hit_white_pixel == true && room_map.at<uchar>(y - 1, x) == 0)
				{
					// check over black pixel for other black pixels, if none occur a critical point is found
					bool critical_point = true;
					for (int dx = -1; dx <= 1; ++dx)
						if (room_map.at<uchar>(y, std::max(0, std::min(x + dx, room_map.cols - 1))) == 0) // check at side after obstacle
							critical_point = false;

					// if a critical point is found mark the separation, note that this algorithm goes left and right
					// starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
					// behind other obstacles on the same y-value as the critical point
					if (critical_point == true)
					{
						const int ym2 = std::max(0, (int)y - 2);

						// to the left until a black pixel is hit
						for (int dx = -1; x + dx >= 0; --dx)
						{
							uchar &val = cell_map.at<uchar>(y - 1, x + dx);
							if (val == 255 && cell_map.at<uchar>(ym2, x + dx) == 255)
								val = BORDER_PIXEL_VALUE;
							else if (val == 0)
								break;
						}

						// to the right until a black pixel is hit
						for (int dx = 1; x + dx < room_map.cols; ++dx)
						{
							uchar &val = cell_map.at<uchar>(y - 1, x + dx);
							if (val == 255 && cell_map.at<uchar>(ym2, x + dx) == 255)
								val = BORDER_PIXEL_VALUE;
							else if (val == 0)
								break;
						}
					}
				}
			}
		}

		// save the found number of segments and the obstacle end points
		previous_number_of_segments = number_of_segments;
		previous_obstacles_end_x = current_obstacles_end_x;
	}

#ifdef DEBUG_VISUALIZATION
	cv::imshow("cell_map", cell_map);
#endif

	// *********************** II.b) merge too small cells into bigger cells ***********************
	cv::Mat cell_map_labels;
	const int number_of_cells = mergeCells(cell_map, cell_map_labels, min_cell_area, min_cell_width);

	// *********************** III. Find the separated cells. ***********************
	std::vector<std::vector<cv::Point>> cells;
	for (int i = 1; i <= number_of_cells; ++i)
	{
		cv::Mat cell_copy(cell_map_labels == i);
		std::vector<std::vector<cv::Point>> cellsi;
		cv::findContours(cell_copy, cellsi, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
		cells.insert(cells.end(), cellsi.begin(), cellsi.end());
	}

#ifdef DEBUG_VISUALIZATION
//	// testing
//	cv::Mat black_map = cv::Mat::zeros(cell_map.rows, cell_map.cols, cell_map.type());
//	for(size_t i=0; i<cells.size(); ++i)
//	{
//		cv::drawContours(black_map, cells, i, cv::Scalar(127), CV_FILLED);
//		cv::imshow("contours", black_map);
//		cv::waitKey();
//	}
#endif

	// create generalized Polygons out of the contours to handle the cells
	for (size_t cell = 0; cell < cells.size(); ++cell)
	{
		GeneralizedPolygon current_cell(cells[cell], map_resolution);
		//		std::cout << cell+1 << ": " << current_cell.getArea() << std::endl;
		if (current_cell.getArea() >= min_cell_area)
		{
			cell_polygons.push_back(current_cell);
			polygon_centers.push_back(current_cell.getCenter());
		}
		else
		{
			std::cout << "WARN: BoustrophedonExplorer::computeCellDecomposition: dropped cell " << cell + 1 << " with area=" << current_cell.getArea() << ". This should only happen for small unconnected cells." << std::endl;
		}
	}
}

int BoustrophedonExplorer::mergeCells(cv::Mat &cell_map, cv::Mat &cell_map_labels, const double min_cell_area, const int min_cell_width)
{
	// label all cells
	//   --> create a label map with 0=walls/obstacles, -1=cell borders, 1,2,3,4...=cell labels
	cell_map.convertTo(cell_map_labels, CV_32SC1, 256, 0);
	//   --> re-assign the cell borders with -1
	for (int v = 0; v < cell_map_labels.rows; ++v)
		for (int u = 0; u < cell_map_labels.cols; ++u)
			if (cell_map_labels.at<int>(v, u) == BORDER_PIXEL_VALUE * 256)
				cell_map_labels.at<int>(v, u) = -1;
	//   --> flood fill cell regions with unique id labels
	std::map<int, boost::shared_ptr<BoustrophedonCell>> cell_index_mapping; // maps each cell label --> to the cell object
	int label_index = 1;
	for (int v = 0; v < cell_map_labels.rows; ++v)
	{
		for (int u = 0; u < cell_map_labels.cols; ++u)
		{
			// if the map has already received a label for that pixel --> skip
			if (cell_map_labels.at<int>(v, u) != 65280)
				continue;

			// fill each cell with a unique id
			cv::Rect bounding_box;
			const double area = cv::floodFill(cell_map_labels, cv::Point(u, v), label_index, &bounding_box, 0, 0, 4);
			cell_index_mapping[label_index] = boost::shared_ptr<BoustrophedonCell>(new BoustrophedonCell(label_index, area, bounding_box));
			label_index++;
			if (label_index == INT_MAX)
				std::cout << "WARN: BoustrophedonExplorer::mergeCells: label_index exceeds range of int." << std::endl;
		}
	}
	std::cout << "INFO: BoustrophedonExplorer::mergeCells: found " << label_index - 1 << " cells before merging." << std::endl;

	// determine the neighborhood relationships between all cells
	for (int v = 1; v < cell_map_labels.rows - 1; ++v)
	{
		for (int u = 1; u < cell_map_labels.cols - 1; ++u)
		{
			if (cell_map_labels.at<int>(v, u) == -1) // only check the border points for neighborhood relationships
			{
				const int label_left = cell_map_labels.at<int>(v, u - 1);
				const int label_right = cell_map_labels.at<int>(v, u + 1);
				if (label_left > 0 && label_right > 0)
				{
					cell_index_mapping[label_left]->neighbors_.insert(cell_index_mapping[label_right]);
					cell_index_mapping[label_right]->neighbors_.insert(cell_index_mapping[label_left]);
				}
				const int label_up = cell_map_labels.at<int>(v - 1, u);
				const int label_down = cell_map_labels.at<int>(v + 1, u);
				if (label_up > 0 && label_down > 0)
				{
					cell_index_mapping[label_up]->neighbors_.insert(cell_index_mapping[label_down]);
					cell_index_mapping[label_down]->neighbors_.insert(cell_index_mapping[label_up]);
				}
			}
		}
	}
#ifdef DEBUG_VISUALIZATION
//	printCells(cell_index_mapping);
//	cv::imshow("cell_map",cell_map);
//	cv::waitKey();
#endif

	// iteratively merge cells
	mergeCellsSelection(cell_map, cell_map_labels, cell_index_mapping, min_cell_area, min_cell_width);

	// re-assign area labels to 1,2,3,4,...
	int new_cell_label = 1;
	for (std::map<int, boost::shared_ptr<BoustrophedonCell>>::iterator itc = cell_index_mapping.begin(); itc != cell_index_mapping.end(); ++itc, ++new_cell_label)
		for (int v = 0; v < cell_map_labels.rows; ++v)
			for (int u = 0; u < cell_map_labels.cols; ++u)
				if (cell_map_labels.at<int>(v, u) == itc->second->label_)
					cell_map_labels.at<int>(v, u) = new_cell_label;

	std::cout << "INFO: BoustrophedonExplorer::mergeCells: " << cell_index_mapping.size() << " cells remaining after merging." << std::endl;
	return cell_index_mapping.size();
}

void BoustrophedonExplorer::mergeCellsSelection(cv::Mat &cell_map, cv::Mat &cell_map_labels, std::map<int, boost::shared_ptr<BoustrophedonCell>> &cell_index_mapping,
												const double min_cell_area, const int min_cell_width)
{
	// iteratively merge cells
	// merge small cells below min_cell_area with their largest neighboring cell
	std::multimap<double, boost::shared_ptr<BoustrophedonCell>> area_to_region_id_mapping; // maps the area of each cell --> to the respective cell
	for (std::map<int, boost::shared_ptr<BoustrophedonCell>>::iterator itc = cell_index_mapping.begin(); itc != cell_index_mapping.end(); ++itc)
		area_to_region_id_mapping.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell>>(itc->second->area_, itc->second));
	for (std::multimap<double, boost::shared_ptr<BoustrophedonCell>>::iterator it = area_to_region_id_mapping.begin(); it != area_to_region_id_mapping.end();)
	{
		// skip if segment is large enough (area and side length criteria)
		if (it->first >= min_cell_area && it->second->bounding_box_.width >= min_cell_width && it->second->bounding_box_.height >= min_cell_width)
		{
			++it;
			continue;
		}

		// skip segments which have no neighbors
		if (it->second->neighbors_.size() == 0)
		{
			std::cout << "WARN: BoustrophedonExplorer::mergeCells: skipping small cell without neighbors." << std::endl;
			++it;
			continue;
		}

		// determine the largest neighboring cell
		const BoustrophedonCell &small_cell = *(it->second);
		std::multimap<double, boost::shared_ptr<BoustrophedonCell>, std::greater<double>> area_sorted_neighbors;
		for (BoustrophedonCell::BoustrophedonCellSetIterator itn = small_cell.neighbors_.begin(); itn != small_cell.neighbors_.end(); ++itn)
			area_sorted_neighbors.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell>>((*itn)->area_, *itn));
		BoustrophedonCell &large_cell = *(area_sorted_neighbors.begin()->second);

		// merge the cells
		mergeTwoCells(cell_map, cell_map_labels, small_cell, large_cell, cell_index_mapping);

		// update area_to_region_id_mapping
		area_to_region_id_mapping.clear();
		for (std::map<int, boost::shared_ptr<BoustrophedonCell>>::iterator itc = cell_index_mapping.begin(); itc != cell_index_mapping.end(); ++itc)
			area_to_region_id_mapping.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell>>(itc->second->area_, itc->second));
		it = area_to_region_id_mapping.begin();

#ifdef DEBUG_VISUALIZATION
//		printCells(cell_index_mapping);
//		cv::imshow("cell_map",cell_map);
//		cv::waitKey();
#endif
	}

	// label remaining border pixels with label of largest neighboring region label
	for (int v = 1; v < cell_map.rows - 1; ++v)
	{
		for (int u = 1; u < cell_map.cols - 1; ++u)
		{
			if (cell_map.at<uchar>(v, u) == BORDER_PIXEL_VALUE)
			{
				std::set<int> neighbor_labels;
				for (int dv = -1; dv <= 1; ++dv)
				{
					for (int du = -1; du <= 1; ++du)
					{
						const int &val = cell_map_labels.at<int>(v + dv, u + du);
						if (val > 0)
							neighbor_labels.insert(val);
					}
				}
				if (neighbor_labels.size() > 0)
				{
					int new_label = -1;
					for (std::multimap<double, boost::shared_ptr<BoustrophedonCell>>::reverse_iterator it = area_to_region_id_mapping.rbegin(); it != area_to_region_id_mapping.rend(); ++it)
					{
						if (neighbor_labels.find(it->second->label_) != neighbor_labels.end())
						{
							cell_map_labels.at<int>(v, u) = it->second->label_;
							break;
						}
					}
				}
				else
					std::cout << "WARN: BoustrophedonExplorer::mergeCells: border pixel has no labeled neighbors." << std::endl;
			}
		}
	}
}

void BoustrophedonExplorer::mergeTwoCells(cv::Mat &cell_map, cv::Mat &cell_map_labels, const BoustrophedonCell &minor_cell, BoustrophedonCell &major_cell,
										  std::map<int, boost::shared_ptr<BoustrophedonCell>> &cell_index_mapping)
{
	// execute merging the minor cell into the major cell
	//   --> remove border from maps
	for (int v = 0; v < cell_map.rows; ++v)
		for (int u = 0; u < cell_map.cols; ++u)
			if (cell_map.at<uchar>(v, u) == BORDER_PIXEL_VALUE &&
				((cell_map_labels.at<int>(v, u - 1) == minor_cell.label_ && cell_map_labels.at<int>(v, u + 1) == major_cell.label_) ||
				 (cell_map_labels.at<int>(v, u - 1) == major_cell.label_ && cell_map_labels.at<int>(v, u + 1) == minor_cell.label_) ||
				 (cell_map_labels.at<int>(v - 1, u) == minor_cell.label_ && cell_map_labels.at<int>(v + 1, u) == major_cell.label_) ||
				 (cell_map_labels.at<int>(v - 1, u) == major_cell.label_ && cell_map_labels.at<int>(v + 1, u) == minor_cell.label_)))
			{
				cell_map.at<uchar>(v, u) = 255;
				cell_map_labels.at<int>(v, u) = major_cell.label_;
				major_cell.area_ += 1;
			}
	//   --> update old label in cell_map_labels
	for (int v = 0; v < cell_map_labels.rows; ++v)
		for (int u = 0; u < cell_map_labels.cols; ++u)
			if (cell_map_labels.at<int>(v, u) == minor_cell.label_)
				cell_map_labels.at<int>(v, u) = major_cell.label_;
	//   --> update major_cell
	major_cell.area_ += minor_cell.area_;
	for (BoustrophedonCell::BoustrophedonCellSetIterator itn = major_cell.neighbors_.begin(); itn != major_cell.neighbors_.end(); ++itn)
		if ((*itn)->label_ == minor_cell.label_)
		{
			major_cell.neighbors_.erase(itn);
			break;
		}
	for (BoustrophedonCell::BoustrophedonCellSetIterator itn = minor_cell.neighbors_.begin(); itn != minor_cell.neighbors_.end(); ++itn)
		if ((*itn)->label_ != major_cell.label_)
			major_cell.neighbors_.insert(*itn);

	// clean all references to minor_cell
	cell_index_mapping.erase(minor_cell.label_);
	for (std::map<int, boost::shared_ptr<BoustrophedonCell>>::iterator itc = cell_index_mapping.begin(); itc != cell_index_mapping.end(); ++itc)
		for (BoustrophedonCell::BoustrophedonCellSetIterator itn = itc->second->neighbors_.begin(); itn != itc->second->neighbors_.end(); ++itn)
			if ((*itn)->label_ == minor_cell.label_)
			{
				(*itn)->label_ = major_cell.label_;
				break;
			}
}

void BoustrophedonExplorer::correctThinWalls(cv::Mat &room_map)
{
	for (int v = 1; v < room_map.rows; ++v)
	{
		for (int u = 1; u < room_map.cols; ++u)
		{
			if (room_map.at<uchar>(v - 1, u - 1) == 255 && room_map.at<uchar>(v - 1, u) == 0 && room_map.at<uchar>(v, u - 1) == 0 && room_map.at<uchar>(v, u) == 255)
				room_map.at<uchar>(v, u) = 0;
			else if (room_map.at<uchar>(v - 1, u - 1) == 0 && room_map.at<uchar>(v - 1, u) == 255 && room_map.at<uchar>(v, u - 1) == 255 && room_map.at<uchar>(v, u) == 0)
				room_map.at<uchar>(v, u - 1) = 0;
		}
	}
}

// 从每一个cell_polygon中搜索路径
void BoustrophedonExplorer::computeBoustrophedonPath(const cv::Mat &room_map, const float map_resolution,
													 const GeneralizedPolygon &cell,
													 int cell_index,
													 std::vector<cv::Point2f> &fov_middlepoint_path,
													 cv::Point &robot_pos,
													 const int grid_spacing_as_int, const int half_grid_spacing_as_int,
													 const double path_eps, const int max_deviation_from_track,
													 const int grid_obstacle_offset,
													 const float min_line_cleaning_distance, const int index_to_boundary)
{
	// get a map that has only the current cell drawn in
	//	Remark:	single cells are obstacle free so it is sufficient to use the cell to check
	// if a position can be reached during the
	//			execution of the coverage path
	// 单一的cell_polgon是没有障碍物的
	cv::Mat cell_map;
	cell.drawPolygon(cell_map, cv::Scalar(255));

	// align the longer dimension of the cell horizontally with the x-axis
	cv::Point cell_center = cell.getBoundingBoxCenter(); //中心点
	cv::Mat R_cell;
	cv::Rect cell_bbox;
	cv::Mat rotated_cell_map;
	RoomRotator cell_rotation;

	// compute the affine rotation matrix for rotating a room into parallel alignment with x-axis (longer side of the room is aligned with x-axis)
	// R is the transform
	// bounding_rect is the ROI of the warped image
	cell_rotation.computeRoomRotationMatrix(cell_map, R_cell, cell_bbox, map_resolution, &cell_center);
	cell_rotation.rotateRoom(cell_map, rotated_cell_map, R_cell, cell_bbox);

	// create inflated obstacles room map and rotate according to cell
	//  --> used later for checking accessibility of Boustrophedon path inside the cell
	// 对cell_polygon进行处理
	cv::Mat inflated_room_map, rotated_inflated_room_map;
	// 腐蚀
	cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1), half_grid_spacing_as_int + grid_obstacle_offset);
	cell_rotation.rotateRoom(inflated_room_map, rotated_inflated_room_map, R_cell, cell_bbox);

	cv::Mat rotated_inflated_cell_map = rotated_cell_map.clone();
	for (int v = 0; v < rotated_inflated_cell_map.rows; ++v)
		for (int u = 0; u < rotated_inflated_cell_map.cols; ++u)
			if (rotated_inflated_cell_map.at<uchar>(v, u) != 0 && rotated_inflated_room_map.at<uchar>(v, u) == 0)
			{
				rotated_inflated_cell_map.at<uchar>(v, u) = 128;
			}

#ifdef DEBUG_VISUALIZATION
	cv::imshow("rotated_cell_map_with_inflation", rotated_inflated_cell_map);
#endif
	// cv::imshow("rotated_cell_map_with_inflation", rotated_inflated_cell_map);
	// cv::waitKey(1000);

	// step 1: 搜索出该区域的直线
	// compute the basic Boustrophedon grid lines
	BoustrophedonGrid grid_lines;

	GridGenerator::generateBoustrophedonGrid(rotated_cell_map, rotated_inflated_cell_map, -1, grid_lines, cv::Vec4i(-1, -1, -1, -1), //cv::Vec4i(min_x, max_x, min_y, max_y),
											 grid_spacing_as_int, half_grid_spacing_as_int, 1, max_deviation_from_track, index_to_boundary);

	std::cout << "grid_lines.size()   " << grid_lines.size() << std::endl;

	// auto grid_line_tmp = grid_lines;
	// grid_lines.clear();
	// for (int i = 5; i < grid_line_tmp.size(); i++)
	// {
	// 	grid_lines.push_back(grid_line_tmp[i]);
	// }

	// for (int i = 0; i < 20; i++)
	// {
	// 	auto point_tmp = grid_lines.front().upper_line[i];
	// 	std::cout << "point_tmp  " << point_tmp.x << "  " << point_tmp.y << std::endl;
	// }

#ifdef DEBUG_VISUALIZATION
	cv::Mat rotated_cell_map_disp = rotated_cell_map.clone();
	for (size_t i = 0; i < grid_lines.size(); ++i)
	{
		for (size_t j = 0; j + 1 < grid_lines[i].upper_line.size(); ++j)
		{
			cv::circle(rotated_cell_map_disp, grid_lines[i].upper_line[j], 1, cv::Scalar(64), CV_FILLED);
			cv::line(rotated_cell_map_disp, grid_lines[i].upper_line[j], grid_lines[i].upper_line[j + 1], cv::Scalar(128), 1);
		}
		for (size_t j = 0; j + 1 < grid_lines[i].lower_line.size(); ++j)
		{
			cv::circle(rotated_cell_map_disp, grid_lines[i].lower_line[j], 1, cv::Scalar(64), CV_FILLED);
			cv::line(rotated_cell_map_disp, grid_lines[i].lower_line[j], grid_lines[i].lower_line[j + 1], cv::Scalar(196), 1);
		}
	}
	cv::imshow("rotated_cell_map", rotated_cell_map_disp);
	cv::waitKey();
#endif

	// if no edge could be found in the cell (e.g. if it is too small), ignore it
	if (grid_lines.size() == 0)
	{
		return;
	}

	// 对每个区域中的每条 grid_lines 进行长度的计算
	for (size_t i = 0; i < grid_lines.size(); ++i)
	{

		int start_x = grid_lines[i].upper_line.front().x;
		int start_y = grid_lines[i].upper_line.front().y;
		int goal_x = grid_lines[i].upper_line.back().x;
		int goal_y = grid_lines[i].upper_line.back().y;

		float distance = map_resolution * hypotf((goal_y - start_y), (goal_x - start_x));
		if (distance < min_line_cleaning_distance)
		// if (distance < 3)
		{
			grid_lines.erase(grid_lines.begin() + i);
		}
		// std::cout << "min_line_cleaning_distance  " << min_line_cleaning_distance << std::endl;
		std::cout << "distance  " << distance << std::endl;
	}
	std::cout << "larger than cleaning distance, grid_lines.size() " << grid_lines.size() << std::endl;

	cv::Mat R_cell_inv_before;
	cv::invertAffineTransform(R_cell, R_cell_inv_before); // invert the rotation matrix to remap the determined points to the original cell

	// 加入 gird_lines_all   // 都标记为false
	for (int i = 0; i < grid_lines.size(); i++)
	{
		auto line_tmp = grid_lines[i];

		std::tuple<bool, int, BoustrophedonLine> tuple_tmp(false, cell_index, line_tmp);
		std::tuple<int, cv::Mat> tuple_R_tmp(cell_index, R_cell_inv_before);

		gird_lines_all.push_back(tuple_tmp);
		gird_lines_all_R_cell_inv.push_back(tuple_R_tmp);
	}

	// 近三退二需要起码3条grid_line
	if (grid_lines.size() < 3)
	{
		std::cout << "grid_lines.size() < 3" << std::endl;
		return;
	}

	// // 可视化
	// // 每个区域的路径可视化
	// cv::Mat fov_path_map = rotated_cell_map.clone();
	// for (size_t i = 0; i < grid_lines.size(); ++i)
	// {
	// 	for (size_t j = 0; j + 1 < grid_lines[i].upper_line.size(); ++j)
	// 	{
	// 		cv::Point p2(grid_lines[i].upper_line[j].x, grid_lines[i].upper_line[j].y);
	// 		cv::circle(fov_path_map, p2, 2, cv::Scalar(0), CV_FILLED);
	// 		cv::imshow("rotated_cell_map_disp", fov_path_map);
	// 		cv::waitKey(1);
	// 	}
	// }
	// get the edge nearest to the current robot position to start the boustrophedon path at,
	//  by looking at the
	// upper and lower horizontal path (possible nearest locations)
	// for the edges transformed to the original coordinates (easier)
	std::vector<cv::Point> outer_corners(4);
	outer_corners[0] = grid_lines[0].upper_line[0];			// upper left corner
	outer_corners[1] = grid_lines[0].upper_line.back();		// upper right corner
	outer_corners[2] = grid_lines.back().upper_line[0];		// lower left corner
	outer_corners[3] = grid_lines.back().upper_line.back(); // lower right corner
	cv::Mat R_cell_inv;
	cv::invertAffineTransform(R_cell, R_cell_inv); // invert the rotation matrix to remap the determined points to the original cell
	cv::transform(outer_corners, outer_corners, R_cell_inv);
	double min_corner_dist = path_planner_.planPath(room_map, robot_pos, outer_corners[0], 1.0, 0.0, map_resolution);
	int min_corner_index = 0;
	// 使用A*搜索最短的路径
	for (int i = 1; i < 4; ++i)
	{
		double dist = path_planner_.planPath(room_map, robot_pos, outer_corners[i], 1.0, 0.0, map_resolution);
		if (dist < min_corner_dist)
		{
			min_corner_dist = dist;
			min_corner_index = i;
		}
	}
	bool start_from_upper_path = (min_corner_index < 2 ? true : false);
	bool start_from_left = (min_corner_index % 2 == 0 ? true : false); // boolean to determine on which side the path should start and to check where the path ended

#ifdef DEBUG_VISUALIZATION
	cv::Mat room_map_disp = room_map.clone();
	for (size_t i = 0; i < outer_corners.size(); i += 2)
		cv::line(room_map_disp, outer_corners[i], outer_corners[i + 1], cv::Scalar(128), 1);
	cv::circle(room_map_disp, robot_pos, 3, cv::Scalar(160), CV_FILLED);
	if (start_from_upper_path == true)
	{
		if (start_from_left == true)
			cv::circle(room_map_disp, outer_corners[0], 3, cv::Scalar(64), CV_FILLED);
		else
			cv::circle(room_map_disp, outer_corners[1], 3, cv::Scalar(64), CV_FILLED);
	}
	else
	{
		if (start_from_left == true)
			cv::circle(room_map_disp, outer_corners[2], 3, cv::Scalar(64), CV_FILLED);
		else
			cv::circle(room_map_disp, outer_corners[3], 3, cv::Scalar(64), CV_FILLED);
	}
	cv::imshow("rotated_room_map", room_map_disp);
#endif

	// connect the boustrophedon paths
	// 决定清扫的顺序
	cv::Point cell_robot_pos;
	bool start = true;
	std::vector<cv::Point> current_fov_path;
	if (start_from_upper_path == true) // plan the path starting from upper horizontal line
	{
		for (BoustrophedonGrid::iterator line = grid_lines.begin(); line != grid_lines.end(); ++line)
		{
			if (start == true) // at the beginning of path planning start at first horizontal line --> no vertical points between lines
			{
				if (start_from_left == true)
					cell_robot_pos = line->upper_line[0];
				else
					cell_robot_pos = line->upper_line.back();
				start = false;
			}

			if (start_from_left == true) // plan path from left to right corner
			{
				// get points on transition between horizontal lines by using the Astar-path
				std::vector<cv::Point> astar_path;
				path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, line->upper_line[0],
									   1.0, 0.0, map_resolution, 0, &astar_path);
				// 按照距离简化
				downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps);

				// get points between left and right corner
				downsamplePath(line->upper_line, current_fov_path, cell_robot_pos, path_eps);

				// add the lower path of the current line if available (and then start from the left side again next time)
				if (line->has_two_valid_lines == true)
					downsamplePathReverse(line->lower_line, current_fov_path, cell_robot_pos, path_eps);
				else
					start_from_left = false; // start from the right side next time
			}
			else // plan path from right to left corner
			{
				// get points on transition between horizontal lines by using the Astar-path
				std::vector<cv::Point> astar_path;
				path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, line->upper_line.back(), 1.0, 0.0, map_resolution, 0, &astar_path);
				downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps);

				// get points between right and left corner
				downsamplePathReverse(line->upper_line, current_fov_path, cell_robot_pos, path_eps);

				// add the lower path of the current line if available (and then start from the right side again next time)
				if (line->has_two_valid_lines == true)
					downsamplePath(line->lower_line, current_fov_path, cell_robot_pos, path_eps);
				else
					start_from_left = true; // start from the left side next time
			}
		}
	}
	else // plan the path from the lower horizontal line
	{
		for (BoustrophedonGrid::reverse_iterator line = grid_lines.rbegin(); line != grid_lines.rend(); ++line)
		{
			if (start == true) // at the beginning of path planning start at first horizontal line --> no vertical points between lines
			{
				if (start_from_left == true)
					cell_robot_pos = line->upper_line[0];
				else
					cell_robot_pos = line->upper_line.back();
				start = false;
			}

			if (start_from_left == true) // plan path from left to right corner
			{
				// get points on transition between horizontal lines by using the Astar-path
				std::vector<cv::Point> astar_path;
				path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, line->upper_line[0], 1.0, 0.0, map_resolution, 0, &astar_path);
				downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps);

				// get points between left and right corner
				downsamplePath(line->upper_line, current_fov_path, cell_robot_pos, path_eps);

				// add the lower path of the current line if available (and then start from the left side again next time)
				if (line->has_two_valid_lines == true)
					downsamplePathReverse(line->lower_line, current_fov_path, cell_robot_pos, path_eps);
				else
					start_from_left = false; // start from the right side next time
			}
			else // plan path from right to left corner
			{
				// get points on transition between horizontal lines by using the Astar-path
				std::vector<cv::Point> astar_path;
				path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, line->upper_line.back(), 1.0, 0.0, map_resolution, 0, &astar_path);
				downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps);

				// get points between right and left corner
				downsamplePathReverse(line->upper_line, current_fov_path, cell_robot_pos, path_eps);

				// add the lower path of the current line if available (and then start from the right side again next time)
				if (line->has_two_valid_lines == true)
					downsamplePath(line->lower_line, current_fov_path, cell_robot_pos, path_eps);
				else
					start_from_left = true; // start from the left side next time
			}
		}
	}
#ifdef DEBUG_VISUALIZATION
	cv::Mat rotated_cell_fov_path_disp = rotated_cell_map.clone();
	for (size_t i = 1; i < current_fov_path.size(); ++i)
	{
		cv::circle(rotated_cell_fov_path_disp, current_fov_path[i], 1, cv::Scalar(196), 1);
		cv::line(rotated_cell_fov_path_disp, current_fov_path[i - 1], current_fov_path[i], cv::Scalar(128), 1);
	}
	cv::imshow("rotated_cell_fov_path", rotated_cell_fov_path_disp);
#endif

	// 进3退2,规划器
	if (1)
	{
		current_fov_path.clear();
		int line_index = 0;
		int line_index_last = line_index;
		int j = 0;
		std::vector<bool> line_has_visited;
		for (int i = 0; i < grid_lines.size(); i++)
		{
			line_has_visited.push_back(false);
		}

		while (line_index < grid_lines.size())
		{
			grid_lines[line_index];
			line_index_last = line_index;

			// 判断首尾和机器人距离的远近
			cv::Point point_start = grid_lines[line_index].upper_line.front();
			cv::Point point_end = grid_lines[line_index].upper_line.back();

			float distance_start = map_resolution * hypotf((robot_pos.y - point_start.y), (robot_pos.x - point_start.x));
			float distance_end = map_resolution * hypotf((robot_pos.y - point_end.y), (robot_pos.x - point_end.x));
			std::cout << "distance_start   " << distance_start << std::endl;
			std::cout << "distance_end   " << distance_end << std::endl;
			if ((j % 2) == 0) //
			// 大于最小的转弯半径
			// if ((distance_end <= distance_start) && (distance_end > 1))
			{ // 从尾加到头
				for (int i = grid_lines[line_index].upper_line.size() - 1; i >= 0; i--)
				{
					cv::Point point_tmp;
					point_tmp.x = grid_lines[line_index].upper_line[i].x;
					point_tmp.y = grid_lines[line_index].upper_line[i].y;
					// 更新机器人位置
					robot_pos.x = point_tmp.x;
					robot_pos.y = point_tmp.y;
					current_fov_path.push_back(point_tmp);
				}
			}
			// else if ((distance_end > distance_start) && (distance_start > 1))
			else
			{ // 从头加到尾
				for (int i = 0; i < grid_lines[line_index].upper_line.size(); i++)
				{
					cv::Point point_tmp;
					point_tmp.x = grid_lines[line_index].upper_line[i].x;
					point_tmp.y = grid_lines[line_index].upper_line[i].y;
					// 更新机器人位置
					robot_pos.x = point_tmp.x;
					robot_pos.y = point_tmp.y;
					current_fov_path.push_back(point_tmp);
				}
			}
			// else
			// {
			// 	continue;
			// }
			// 已经清扫的标记位
			first_cleaning_line_num++;
			line_has_visited[line_index] = true;
			auto current_line = gird_lines_all.end() - grid_lines.size() + line_index;
			std::get<0>(*current_line) = true; //修改tuple的值

			// if ((j % 2) == 0)
			// {
			// 	line_index += 3;
			// }
			// else
			// {
			// 	line_index -= 2;
			// }

			bool has_cleaned_all = true;
			for (int k = 0; k <= line_index; k++)
			{
				has_cleaned_all = has_cleaned_all && line_has_visited[k];
			}
			if (has_cleaned_all)
			{
				line_index = line_index_last + 3;
			}
			else
			{
				line_index = line_index_last - 2;
			}

			std::cout << "line_index  " << line_index << std::endl;

			j++;
		}
	}
	// remap the fov path to the originally rotated cell and add the found points to the global path
	std::vector<cv::Point2f> fov_middlepoint_path_part;
	for (std::vector<cv::Point>::iterator point = current_fov_path.begin(); point != current_fov_path.end(); ++point)
		fov_middlepoint_path_part.push_back(cv::Point2f(point->x, point->y));
	cv::transform(fov_middlepoint_path_part, fov_middlepoint_path_part, R_cell_inv);
	fov_middlepoint_path.insert(fov_middlepoint_path.end(), fov_middlepoint_path_part.begin(), fov_middlepoint_path_part.end());

#ifdef DEBUG_VISUALIZATION
	cv::Mat cell_fov_path_disp = cell_map.clone();
	for (size_t i = 1; i < fov_middlepoint_path.size(); ++i)
	{
		cv::circle(cell_fov_path_disp, fov_middlepoint_path[i], 1, cv::Scalar(196), 1);
		cv::line(cell_fov_path_disp, fov_middlepoint_path[i - 1], fov_middlepoint_path[i], cv::Scalar(128), 1);
	}
	cv::imshow("cell_fov_path", cell_fov_path_disp);
	cv::waitKey(8000);
#endif

	// // 可视化  // 带清扫顺序
	// cv::Mat fov_path_map_1 = cell_map.clone();
	// for (size_t i = 0; i < fov_middlepoint_path.size(); ++i)
	// {
	// 	cv::Point p2(fov_middlepoint_path[i].x, fov_middlepoint_path[i].y);
	// 	cv::circle(fov_path_map_1, p2, 2, cv::Scalar(0), CV_FILLED);
	// 	cv::imshow("cleaning sequence", fov_path_map_1);
	// 	cv::waitKey(1);
	// }

	// also update the current robot position
	std::vector<cv::Point> current_pos_vector(1, cell_robot_pos);
	cv::transform(current_pos_vector, current_pos_vector, R_cell_inv);
	robot_pos = current_pos_vector[0];
	std::cout << "robot_pos   " << robot_pos.x << "   " << robot_pos.y << std::endl;
}

// 补扫的轨迹
void BoustrophedonExplorer::computeBoustrophedonPath_miss_out(const cv::Mat &room_map, const float map_resolution,
															  std::vector<GeneralizedPolygon> cell_polygons,
															  std::vector<int> optimal_order,
															  std::vector<cv::Point2f> &fov_middlepoint_path,
															  cv::Point &robot_pos,
															  const int grid_spacing_as_int, const int half_grid_spacing_as_int,
															  const double path_eps, const int max_deviation_from_track,
															  const int grid_obstacle_offset,
															  const float min_line_cleaning_distance)
{
	// 哪些grid_line已经加入清扫的轨迹,哪些还没有加入
	int i = 0;
	int cleaning_num = 0;
	int miss_out_index = 0;
	// 从后向前搜索
	for (auto iter = gird_lines_all.end() - 1; iter >= gird_lines_all.begin(); iter--)
	{
		// std::get<0>(*current_line)

		// std::cout << "index  " << i << " " << std::get<0>(*iter) << std::endl;
		if (std::get<0>(*iter) == 1)
		{
			cleaning_num++;
		}

		// 没有加入清扫队列  // 对于每一个没有加入清扫对列的
		if (std::get<0>(*iter) == 0)
		{
			// 判断首尾和机器人距离的远近
			cv::Point point_start = std::get<2>(*iter).upper_line.front();
			cv::Point point_end = std::get<2>(*iter).upper_line.back();

			float distance_start = map_resolution * hypotf((robot_pos.y - point_start.y), (robot_pos.x - point_start.x));
			float distance_end = map_resolution * hypotf((robot_pos.y - point_end.y), (robot_pos.x - point_end.x));

			miss_out_index = std::get<1>(*iter);
			std::vector<cv::Point> current_fov_path;

			// 取distance_start和distance_end中大的那一个
			float min_distance = distance_start < distance_end ? distance_start : distance_end;

			int end_start_index = std::get<2>(*iter).upper_line.size() - 1;
			int front_start_index = 0;

			if (min_distance < grid_spacing_as_int * 2)
			{
				// 确保机器人有足够的转弯半径
				for (int p = std::get<2>(*iter).upper_line.size() - 1; p >= 0; p--)
				{
					cv::Point point_tmp;
					point_tmp.x = std::get<2>(*iter).upper_line[p].x;
					point_tmp.y = std::get<2>(*iter).upper_line[p].y;
					float index_distance_tmp = hypotf((robot_pos.y - point_tmp.y), (robot_pos.x - point_tmp.x));
					// 确保机器人有足够的转弯半径
					if (index_distance_tmp > grid_spacing_as_int * 2)
					{
						end_start_index = p;
						break;
					}
				}

				for (int q = 0; q <= std::get<2>(*iter).upper_line.size() - 1; q++)
				{
					cv::Point point_tmp;
					point_tmp.x = std::get<2>(*iter).upper_line[q].x;
					point_tmp.y = std::get<2>(*iter).upper_line[q].y;
					float index_distance_tmp = hypotf((robot_pos.y - point_tmp.y), (robot_pos.x - point_tmp.x));
					// 确保机器人有足够的转弯半径
					if (index_distance_tmp > grid_spacing_as_int * 2)
					{
						front_start_index = q;
						break;
					}
				}
			}

			if (distance_end < distance_start)
			{ // 从尾加到头
				cv::Point point_tmp;
				for (int k = end_start_index; k >= 0; k--)
				{
					point_tmp.x = std::get<2>(*iter).upper_line[k].x;
					point_tmp.y = std::get<2>(*iter).upper_line[k].y;
					float index_distance_tmp = hypotf((robot_pos.y - point_tmp.y), (robot_pos.x - point_tmp.x));
					// 确保机器人有足够的转弯半径
					// if (index_distance_tmp > grid_spacing_as_int * 2)
					{
						current_fov_path.push_back(point_tmp);
					}
				}
				// 更新机器人位置
				robot_pos.x = point_tmp.x;
				robot_pos.y = point_tmp.y;
			}
			else
			{
				// 从头加到尾
				cv::Point point_tmp;
				for (int k = front_start_index; k <= std::get<2>(*iter).upper_line.size() - 1; k++)
				{

					point_tmp.x = std::get<2>(*iter).upper_line[k].x;
					point_tmp.y = std::get<2>(*iter).upper_line[k].y;
					float index_distance_tmp = hypotf((robot_pos.y - point_tmp.y), (robot_pos.x - point_tmp.x));
					// 确保机器人有足够的转弯半径
					// if (index_distance_tmp > grid_spacing_as_int * 2)
					{
						current_fov_path.push_back(point_tmp);
					}
				}
				// 更新机器人位置
				robot_pos.x = point_tmp.x;
				robot_pos.y = point_tmp.y;
			}

			// remap the fov path to the originally rotated cell and add the found points to the global path
			std::vector<cv::Point2f> fov_middlepoint_path_part;
			for (std::vector<cv::Point>::iterator point = current_fov_path.begin(); point != current_fov_path.end(); ++point)
			{
				fov_middlepoint_path_part.push_back(cv::Point2f(point->x, point->y));
			}

			cv::Mat R_cell_inv;
			for (int m = 0; m < gird_lines_all_R_cell_inv.size(); m++)
			{
				if (std::get<0>(gird_lines_all_R_cell_inv[m]) == miss_out_index)
				{
					R_cell_inv = std::get<1>(gird_lines_all_R_cell_inv[m]);
				}
			}

			cv::transform(fov_middlepoint_path_part, fov_middlepoint_path_part, R_cell_inv);
			// fov_middlepoint_path
			fov_middlepoint_path.insert(fov_middlepoint_path.end(), fov_middlepoint_path_part.begin(), fov_middlepoint_path_part.end());
		}

		i++;
	}
	std::cout << "first_cleaning_line_num   " << first_cleaning_line_num << std::endl;
	std::cout << "cleaning_num   " << cleaning_num << std::endl;
}

void BoustrophedonExplorer::downsamplePath(const std::vector<cv::Point> &original_path,
										   std::vector<cv::Point> &downsampled_path,
										   cv::Point &robot_pos, const double path_eps)
{
	// downsample path
	for (size_t path_point = 0; path_point < original_path.size(); ++path_point)
	{
		if (cv::norm(robot_pos - original_path[path_point]) >= path_eps)
		{
			downsampled_path.push_back(original_path[path_point]);
			robot_pos = original_path[path_point];
		}
	}
	// add last element
	if (original_path.size() > 0)
	{
		downsampled_path.push_back(original_path.back());
		robot_pos = original_path.back();
	}
}

void BoustrophedonExplorer::downsamplePathReverse(const std::vector<cv::Point> &original_path, std::vector<cv::Point> &downsampled_path,
												  cv::Point &robot_pos, const double path_eps)
{
	// downsample path
	for (size_t path_point = original_path.size() - 1;; --path_point)
	{
		if (cv::norm(robot_pos - original_path[path_point]) >= path_eps)
		{
			downsampled_path.push_back(original_path[path_point]);
			robot_pos = original_path[path_point];
		}
		if (path_point == 0)
			break;
	}
	// add last element
	if (original_path.size() > 0)
	{
		downsampled_path.push_back(original_path[0]);
		robot_pos = original_path[0];
	}
}

void BoustrophedonExplorer::printCells(std::map<int, boost::shared_ptr<BoustrophedonCell>> &cell_index_mapping)
{
	std::cout << "---\n";
	for (std::map<int, boost::shared_ptr<BoustrophedonCell>>::iterator itc = cell_index_mapping.begin(); itc != cell_index_mapping.end(); ++itc)
	{
		std::cout << itc->first << ": l=" << itc->second->label_ << "   a=" << itc->second->area_ << "   n=";
		for (BoustrophedonCell::BoustrophedonCellSetIterator its = itc->second->neighbors_.begin(); its != itc->second->neighbors_.end(); ++its)
			std::cout << (*its)->label_ << ", ";
		std::cout << std::endl;
	}
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// BoustrophedonVariantExplorer
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void BoustrophedonVariantExplorer::mergeCellsSelection(cv::Mat &cell_map, cv::Mat &cell_map_labels, std::map<int, boost::shared_ptr<BoustrophedonCell>> &cell_index_mapping,
													   const double min_cell_area, const int min_cell_width)
{
	// iteratively merge cells
	//todo:
	// - take one major cell (the largest) and its major direction
	// - merge every other cell into the major cell, except
	//   - the width along the major direction is too small and the cell is sufficiently large
	//   - the bounding box orientation (side length ratio) deviates strongly from the major direction
	//   - the cell main direction is not well aligned with the major direction (skew, 90 deg)

	RoomRotator room_rotator;
	//double rotation_angle = room_rotator.computeRoomMainDirection(cell_map, map_resolution);

	// merge small cells below min_cell_area with their largest neighboring cell
	std::multimap<double, boost::shared_ptr<BoustrophedonCell>> area_to_region_id_mapping; // maps the area of each cell --> to the respective cell
	for (std::map<int, boost::shared_ptr<BoustrophedonCell>>::iterator itc = cell_index_mapping.begin(); itc != cell_index_mapping.end(); ++itc)
		area_to_region_id_mapping.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell>>(itc->second->area_, itc->second));
	for (std::multimap<double, boost::shared_ptr<BoustrophedonCell>>::iterator it = area_to_region_id_mapping.begin(); it != area_to_region_id_mapping.end();)
	{
		// abort if no cells below min_cell_area remain unmerged into bigger cells
		if (it->first >= min_cell_area && it->second->bounding_box_.width >= min_cell_width && it->second->bounding_box_.height >= min_cell_width)
		{
			++it;
			continue;
		}

		// skip segments which have no neighbors
		if (it->second->neighbors_.size() == 0)
		{
			std::cout << "WARN: BoustrophedonExplorer::mergeCells: skipping small cell without neighbors." << std::endl;
			++it;
			continue;
		}

		// determine the largest neighboring cell
		const BoustrophedonCell &small_cell = *(it->second);
		std::multimap<double, boost::shared_ptr<BoustrophedonCell>, std::greater<double>> area_sorted_neighbors;
		for (BoustrophedonCell::BoustrophedonCellSetIterator itn = small_cell.neighbors_.begin(); itn != small_cell.neighbors_.end(); ++itn)
			area_sorted_neighbors.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell>>((*itn)->area_, *itn));
		BoustrophedonCell &large_cell = *(area_sorted_neighbors.begin()->second);

		// merge the cells
		mergeTwoCells(cell_map, cell_map_labels, small_cell, large_cell, cell_index_mapping);

		// update area_to_region_id_mapping
		area_to_region_id_mapping.clear();
		for (std::map<int, boost::shared_ptr<BoustrophedonCell>>::iterator itc = cell_index_mapping.begin(); itc != cell_index_mapping.end(); ++itc)
			area_to_region_id_mapping.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell>>(itc->second->area_, itc->second));
		it = area_to_region_id_mapping.begin();

#ifdef DEBUG_VISUALIZATION
//		printCells(cell_index_mapping);
//		cv::imshow("cell_map",cell_map);
//		cv::waitKey();
#endif
	}

	// label remaining border pixels with label of largest neighboring region label
	for (int v = 1; v < cell_map.rows - 1; ++v)
	{
		for (int u = 1; u < cell_map.cols - 1; ++u)
		{
			if (cell_map.at<uchar>(v, u) == BORDER_PIXEL_VALUE)
			{
				std::set<int> neighbor_labels;
				for (int dv = -1; dv <= 1; ++dv)
				{
					for (int du = -1; du <= 1; ++du)
					{
						const int &val = cell_map_labels.at<int>(v + dv, u + du);
						if (val > 0)
							neighbor_labels.insert(val);
					}
				}
				if (neighbor_labels.size() > 0)
				{
					int new_label = -1;
					for (std::multimap<double, boost::shared_ptr<BoustrophedonCell>>::reverse_iterator it = area_to_region_id_mapping.rbegin(); it != area_to_region_id_mapping.rend(); ++it)
					{
						if (neighbor_labels.find(it->second->label_) != neighbor_labels.end())
						{
							cell_map_labels.at<int>(v, u) = it->second->label_;
							break;
						}
					}
				}
				else
					std::cout << "WARN: BoustrophedonExplorer::mergeCells: border pixel has no labeled neighbors." << std::endl;
			}
		}
	}
}
